package edu.wpi.first.wpilibj.templates.subsystems;

/**
 * 
 * @author FRC Team 3255
 */

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.*;
import edu.wpi.first.wpilibj.templates.subsystems.*;
import edu.wpi.first.wpilibj.templates.commands.CommandBase;


public class Telemetry extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    
    public Telemetry(){
    }
    
    public void update() {
        // show DriveTrain telemetry
        SmartDashboard.putNumber("Left Speed", CommandBase.myDriveTrain.currentLeftSpeed());
        SmartDashboard.putNumber("Right Speed", CommandBase.myDriveTrain.currentRightSpeed());
        SmartDashboard.putNumber("Encoder", CommandBase.myDriveTrain.getEncoderValue());
        SmartDashboard.putBoolean("High Gear", CommandBase.myDriveTrain.isHighGear());
        SmartDashboard.putNumber("Gyro Angle", CommandBase.myDriveTrain.getGyroAngle());

        // show Collector telemetry
        SmartDashboard.getString("Collector Position", CommandBase.myCollector.getPositionString());
        SmartDashboard.putNumber("Collector Pot Voltage", CommandBase.myCollector.potVoltage());
        SmartDashboard.putNumber("Position Speed", CommandBase.myCollector.getPositionSpeed());
        SmartDashboard.putNumber("Collector Speed", CommandBase.myCollector.getSpeed());
        
        // show Shooter pot telemetry
        SmartDashboard.getString("Shooter Position", CommandBase.myLinearShooter.getPositionString());
        SmartDashboard.putNumber("Shooter Pot Voltage", CommandBase.myLinearShooter.potVoltage());
        SmartDashboard.putNumber("Pitch Error", CommandBase.myLinearShooter.getError());
        SmartDashboard.putNumber("Pitch Speed", CommandBase.myLinearShooter.getPositionSpeed());
        SmartDashboard.putNumber("Pitch Time", CommandBase.oi.GetShooterPitchTime());
        SmartDashboard.putNumber("Shooter Speed", CommandBase.myLinearShooter.currentShooterSpeed());
        SmartDashboard.putBoolean("Photon Cannon", CommandBase.myLinearShooter.photonCannonState());
        
        // show Hopper telemetry
        SmartDashboard.putBoolean("Hopper Light", CommandBase.myHopper.isHopperLightsOn());
        SmartDashboard.putNumber("Hopper Speed", CommandBase.myHopper.getSpeed());
        SmartDashboard.putBoolean("Hopper Start", CommandBase.myHopper.getStartSwitchState());
        SmartDashboard.putBoolean("Hopper Stop", CommandBase.myHopper.getStopSwitchState());
        
        // show Climber telemetry
        SmartDashboard.putBoolean("Climber Deployed", CommandBase.myClimber.isClimberDeployed());
        SmartDashboard.putString("Cimber Position", CommandBase.myClimber.climberPosition());
        
        // show Camera telemetry
        SmartDashboard.putBoolean("Camera Up", CommandBase.myCamera.isLookUp());
        
        SmartDashboard.putNumber("Autonomous Delay", CommandBase.oi.GetProFlightDelay());
        SmartDashboard.putNumber("Autonomous Distance", CommandBase.oi.GetProFlightDistance());
        SmartDashboard.putNumber("Feeder Timeout", CommandBase.oi.GetFeederTimeout());
        SmartDashboard.putNumber("Pickup Timeout", CommandBase.oi.GetPickupTimeout());
    }
    
    public void init() {
        SmartDashboard.putData(CommandBase.myDriveTrain);
        SmartDashboard.putData(CommandBase.myLinearShooter);
        SmartDashboard.putData(CommandBase.myPneumatics);
        SmartDashboard.putData(CommandBase.myHopper);
        SmartDashboard.putData(CommandBase.myCollector);
        SmartDashboard.putData(CommandBase.myClimber);
        SmartDashboard.putData(CommandBase.myCamera);
        
        LiveWindow.addActuator("Collector", "Pitch Motor", CommandBase.myCollector.positionMotor);
        LiveWindow.addSensor("Collector", "Pot", CommandBase.myCollector.pot);
        LiveWindow.addActuator("Collector", "PIDSubsystem Controller", CommandBase.myCollector.getPIDController());
        
        LiveWindow.addActuator("Shooter", "Pitch Motor", CommandBase.myLinearShooter.pitchMotor);
        LiveWindow.addSensor("Shooter", "Pot", CommandBase.myLinearShooter.pot);
        LiveWindow.addActuator("Shooter", "PIDSubsystem Controller", CommandBase.myLinearShooter.getPIDController());
    }
    
    public void initDefaultCommand() {
        this.setDefaultCommand(new DoUpdateTelemetry());
    }
}